#include "encoder.h"

encoder_t encoder_dev;

/**
 * 从定时器获取电机码盘值 10ms更新速度值
 */
void encoder_timer_cnt(void)
{
	int16_t temp = 0;
	int16_t midd = (REDUCTION_RATIO*ENCODER_PCR*4);
	int16_t cnt_diff = 0;

	uint32_t timer_cnt[4] = {
			TIM2 -> CNT,
			TIM3 -> CNT,
			TIM4 -> CNT,
			TIM5 -> CNT
	};

	// M1~4 码盘累计值
	for(uint8_t i = 0; i < 4; i++)
	{
		temp = timer_cnt[i];
		cnt_diff = temp - encoder_dev.encoder_M_last[i];
		encoder_dev.encoder_M_last[i] = temp;
		if(cnt_diff > (midd / 2))
		{
			cnt_diff -= midd;
		}
		else if(cnt_diff < (-midd / 2))
		{
			cnt_diff += midd;
		}

		// 获取码盘累计值
		encoder_dev.encoder_M[i] += cnt_diff;

		// 获取运行方向
		if(cnt_diff > 0)
		{
			encoder_dev.dir[i] = DIR_FORWARD;
		}
		else if(cnt_diff < 0)
		{
			encoder_dev.dir[i] = DIR_BACKWARD;
		}
		else
		{
			encoder_dev.dir[i] = DIR_NONE;
		}

		// 获取速度
		encoder_dev.speed_M[i] = cnt_diff;
	}
}
